// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/float64.hpp"
#include <geometry_msgs/msg/point.hpp>

// 目标角度/位置
float target_j1 = 0.0, target_j2 = 0.0, target_j3 = 0.0;
// 实时角度/行程
float rt_j1 = 0.0, rt_j2 = 0.0, rt_j3 = 0.0;
//
float j1_cbdata = 0.0, j2_cbdata = 0.0, j3_cbdata = 0.0;
//
float j1_icbdata = 0.0, j2_icbdata = 0.0, j3_icbdata = 0.0;

// 3.自定义节点类；
class Arm_Control : public rclcpp::Node
{
public:
    Arm_Control() : Node("arm_control_node_cpp")
    {
        // 创建发布方
        // --向逆解程序发布坐标
        this->tar_x = this->create_publisher<std_msgs::msg::Float64>("x_goat", 5);
        this->tar_y = this->create_publisher<std_msgs::msg::Float64>("y_goat", 5);
        this->tar_z = this->create_publisher<std_msgs::msg::Float64>("z_goat", 5);
        // 创建订阅方
        // 订阅来自编码器和 ADC的数据
        this->joint1_sub = this->create_subscription<std_msgs::msg::Int32>("joint1_p_", 5, std::bind(&Arm_Control::j1_cb, this, std::placeholders::_1));
        this->joint2_sub = this->create_subscription<std_msgs::msg::Float64>("joint2_p_", 5, std::bind(&Arm_Control::j2_cb, this, std::placeholders::_1));
        this->joint3_sub = this->create_subscription<std_msgs::msg::Float64>("joint3_p_", 5, std::bind(&Arm_Control::j3_cb, this, std::placeholders::_1));
        // 订阅逆解求得的数据，角度、行程
        this->j1_sub = this->create_subscription<std_msgs::msg::Float64>("joint1_gp_", 5, std::bind(&Arm_Control::j1_icb, this, std::placeholders::_1));
        this->j2_sub = this->create_subscription<std_msgs::msg::Float64>("joint2_gp_", 5, std::bind(&Arm_Control::j2_icb, this, std::placeholders::_1));
        this->j3_sub = this->create_subscription<std_msgs::msg::Float64>("joint3_gp_", 5, std::bind(&Arm_Control::j3_icb, this, std::placeholders::_1));
        // 订阅传来的x y z坐标
        // 创建一个订阅者，订阅 "point_topic" 主题
        this->subscription_ = create_subscription<geometry_msgs::msg::Point>("point_topic", 10, std::bind(&Arm_Control::point_callback, this, std::placeholders::_1));

        // 添加定时器（ms周期，按需调整）
        check_timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&Arm_Control::check_target_reached, this));
        check_timer_->cancel(); // 默认不启动
    }
    void go_target(const float target_x, const float target_y, const float target_z)
    {
        // 获取坐标
        auto tarv_x = std_msgs::msg::Float64();
        auto tarv_y = std_msgs::msg::Float64();
        auto tarv_z = std_msgs::msg::Float64();
        tarv_x.data = target_x;
        tarv_y.data = target_y;
        tarv_z.data = target_z;
        // 发送坐标
        tar_x->publish(tarv_x);
        tar_y->publish(tarv_y);
        tar_z->publish(tarv_z);
        // 重置标志并启动定时器
        reset_flags();
        check_timer_->reset();
    }

private:
    bool j1cb_updated = false;
    bool j2cb_updated = false;
    bool j3cb_updated = false;
    bool j1icb_updated = false;
    bool j2icb_updated = false;
    bool j3icb_updated = false;
    rclcpp::TimerBase::SharedPtr check_timer_;

    // 目标位置发布
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr tar_x;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr tar_y;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr tar_z;

    // 一号编码器，二三号ADC的数据
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr joint1_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint2_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint3_sub;
    // 逆解求得的数据，角度、行程
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr j1_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr j2_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr j3_sub;
    // 来自其他程序的 x y z 坐标
    rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr subscription_;
    //
    void j1_cb(const std_msgs::msg::Int32::SharedPtr j1_msg)
    {
        j1_cbdata = float(j1_msg->data);
        j1cb_updated = true;
    }
    void j2_cb(const std_msgs::msg::Float64::SharedPtr j2_msg)
    {
        j2_cbdata = j2_msg->data;
        j2cb_updated = true;
    }
    void j3_cb(const std_msgs::msg::Float64::SharedPtr j3_msg)
    {
        j3_cbdata = j3_msg->data;
        j3cb_updated = true;
    }
    //
    void j1_icb(const std_msgs::msg::Float64::SharedPtr j1_imsg)
    {
        j1_icbdata = j1_imsg->data;
        j1icb_updated = true;
    }
    void j2_icb(const std_msgs::msg::Float64::SharedPtr j2_imsg)
    {
        j2_icbdata = j2_imsg->data;
        j2icb_updated = true;
    }
    void j3_icb(const std_msgs::msg::Float64::SharedPtr j3_imsg)
    {
        j3_icbdata = j3_imsg->data;
        j3icb_updated = true;
    }

    void check_target_reached()
    {

        // 只有所有数据更新后才进行检查
        if (!(j1cb_updated && j2cb_updated && j3cb_updated &&
              j1icb_updated && j2icb_updated && j3icb_updated))
        {
            RCLCPP_DEBUG(this->get_logger(), "等待数据更新...");
            return;
        }

        // 计算误差
        const float tolerance = 2;
        float error_j1 = std::abs(j1_cbdata - j1_icbdata);
        float error_j2 = std::abs(j2_cbdata - j2_icbdata);
        float error_j3 = std::abs(j3_cbdata - j3_icbdata);

        if (error_j1 <= tolerance && error_j2 <= tolerance && error_j3 <= tolerance)
        {
            RCLCPP_INFO(this->get_logger(), "目标已到达!");
            check_timer_->cancel(); // 停止定时器
        }
        else
        {
            RCLCPP_INFO_THROTTLE( // 节流输出避免刷屏
                this->get_logger(), *this->get_clock(), 1000,
                "未到达目标，当前误差: j1=%.3f, j2=%.3f, j3=%.3f",
                error_j1, error_j2, error_j3);
        }
        // 重置标志等待下次更新
        reset_flags();
    }
    void reset_flags()
    {
        j1cb_updated = false;
        j2cb_updated = false;
        j3cb_updated = false;
        j1icb_updated = false;
        j2icb_updated = false;
        j3icb_updated = false;
    }

    void point_callback(const geometry_msgs::msg::Point::SharedPtr msg)
    {
        // 打印接收到的点坐标
        RCLCPP_INFO(this->get_logger(), "Received point: x=%.2f, y=%.2f, z=%.2f", msg->x, msg->y, msg->z);
        go_target(msg->x,msg->y,msg->z);
    }
};

int main(int argc, char const *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<Arm_Control>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}